%% Saturn
warning off
clc;clear;close all

format longg
options_negx = odeset('RelTol',1e-13,'AbsTol',1e-13,'Events',@NegXcrossing);
options=odeset('RelTol',1e-13,'AbsTol',1e-13);
options_fmincon = optimoptions('fmincon', 'MaxFunctionEvaluations', 5000, ...
    'MaxIterations', 5000,'ConstraintTolerance',1e-6,'Algorithm','Interior-point','stepTolerance',1e-21);
%% Inputs.
G           = 6.674e-11 * ((1/1000)^3); % Gravitational parameters

mass_central = 1.989e30;
%[] Mass of central planet

mass_moon = 5.972e24;         %Callisto
%[] mass of moons

DU = 151.73e6;           %Callisto
%[km] Semi-major axis of Moons, used as distance units for each CRTBP
%environment.

moonName = {'Luna'};
%[] Name of the Moons

thetao = 0;
%[] Initial position of the moon relative to the first point of aries

GM_central = mass_central*G;
%[] Gravitational parameters

GM_moon = mass_moon*G;
%[] Gravitational parameters

N = length(mass_moon);
%[] Number of Moons

u = zeros(N,1);
for ii = 1:N
    u(ii) = GM_moon(ii)/(GM_moon(ii)+GM_central);
end
%[] Gravataional ratio

TU = zeros(N,1);
VU = zeros(N,1);
for ii = 1:N
    TU(ii) = sqrt(DU(ii)^3/GM_central);
    VU(ii) = DU(ii)/TU(ii);
end
%[] Time constant for each crtbp system

theta_dot = zeros(1,N);
for ii = 1:length(theta_dot)
    theta_dot(ii) = sqrt(GM_central*DU(ii))/DU(ii)^2;
end
%[] Theta dot for each planet.



%% environment settings
dt = 0.01;
startphase=0; % changing the starting position of the periodic
% orbit to show changes in the pole visibility based on the Satellite's position wrt the Sun's pole
% OpenCRTBP_u(u)
%% Define inertial frame FNs
[xs,ys,zs] = ellipsoid(-0,0,0,696340,696340,696340,180);
figure;
s = surface(xs,ys,zs);
rotate(s, [1 0 0], 7.25);
rotate(s, [0 0 1], 73.67+0.014*(2023-1850));
InclinationRotationAngle = 7.25;
RAANRotationAngle = 73.67+0.014*(2023-1850);
PoleDirection = Rotation([0;0;1],RAANRotationAngle,'Degrees')*...
    Rotation([1;0;0],InclinationRotationAngle,'Degrees')*...
    [0;0;1];
rotate(s, PoleDirection, -70);
FN_i = s.VertexNormals;
[row,col,~] = size(FN_i);
%%
load('SE_L4_Vertical.mat');
load('SE_L5_Vertical.mat');
inc_list = zeros(1,length(T_L4_VL));
for ii = 1:length(T_L4_VL)
    IC = IC_L4_VL(:,ii);
    T = T_L4_VL(ii);
    [t,S] = ode113(@(t,S)CR3BP_n(t,S,u),[0:dt:T],IC,options);
    S = S';
    S_i = zeros(size(S));
    S_i = C2I_primary(S(:,ii),u,DU,VU,t(ii));
    COE_list = State2Coe(S_i,GM_central);
    inc_list(ii) = COE_list(3);
end
%%
load('SEL1_Lyapunov.mat');
size = 1;
[t,S] = ode113(@(t,S)CR3BP_n(t,S,u),[0:dt:2*pi],IC(:,size),options);
S=S';
S_i = zeros(6,length(t));
for ii = 1:length(t)
    S_i(:,ii) = C2I_primary(S(:,ii),u,DU,VU,t(ii));
    epoch(ii) = datetime(2023,1,1,0,0,0) + seconds(t(ii)*TU);
end
L1.S_i = S_i;
L1.epoch = epoch;
%% Given orbital states of the satellite (varying inclination), allowable theta_max, and target latitude,
%  Determine visibility

CameraAngleCapability=[40,50,60,70];
target_latitude = -90:90;
orbit_amount = 3;
%%
% for iii = 1:length(CameraAngleCapability)
%     fprintf('CameraAngleCapability=%d \n',CameraAngleCapability(iii))
%     ind = 1;
%     for n = [1:orbit_amount:length(IC_L4_VL),length(IC_L4_VL)]
%         fprintf('%d / %d \n',n,length(IC_L4_VL))
%         L4 = DetermineOptimalL4andL5State(IC_L4_VL(:,n),T_L4_VL(n),u,DU,VU,TU,dt);
%         L5 = DetermineOptimalL4andL5State(IC_L5_VL(:,n),T_L4_VL(n),u,DU,VU,TU,dt);
%         visibility_One_Satellite=zeros(row,col);
%         visibility_Two_Satellite=zeros(row,col);
%         angle=zeros(3,1);
%         parfor ii = 1:row
%             for jj = 1:col
%                 normvec = [FN_i(ii,jj,1);...
%                     FN_i(ii,jj,2);...
%                     FN_i(ii,jj,3)];
%                 for k = 1:length(L4.epoch)
% 
%                     posvec_L4 = L4.S_i(1:3,k);
%                     angle_temp_L4 = acosd(dot(normvec,posvec_L4)/(norm(normvec)*norm(posvec_L4)));
% 
%                     posvec_L5 = L5.S_i(1:3,k);
% %                     angle_temp_L5 = acosd(dot(normvec,posvec_L5)/(norm(normvec)*norm(posvec_L5)));
%                                         angle_temp_L5=100;
% 
%                     posvec_L1 = L1.S_i(1:3,k);
%                     angle_temp_L1 = acosd(dot(normvec,posvec_L1)/(norm(normvec)*norm(posvec_L1)));
% 
%                     if sum((angle_temp_L4< CameraAngleCapability(iii))+...
%                             (angle_temp_L5< CameraAngleCapability(iii))+...
%                             (angle_temp_L1< CameraAngleCapability(iii)))>0
%                         visibility_One_Satellite(ii,jj)=visibility_One_Satellite(ii,jj)+1;
%                     end
%                     if ((angle_temp_L4< CameraAngleCapability(iii))+...
%                             (angle_temp_L5< CameraAngleCapability(iii))+...
%                             (angle_temp_L1< CameraAngleCapability(iii)))==2
%                         visibility_Two_Satellite(ii,jj)=visibility_Two_Satellite(ii,jj)+1;
%                     end
%                 end
%             end
%         end
%         % We need to check how many times the solar surface was visible,
%         % when running this code,
%         % Calculate the averaged visible-day for each target-Latitude.
%         % get index, which depends on the mesh size.
%         for p = 1:length(visibility_Two_Satellite)
%             %             Upper_latitude_One_Satellite = sum(visibility_One_Satellite(91+target_latitude(p),:))/180;
%             %             Lower_latitude_One_Satellite = sum(visibility_One_Satellite(91-target_latitude(p),:))/180;
%             Average_visibility_One_Satellite{iii}(ind,p) = (sum(visibility_One_Satellite(p,:))/180)*dt*TU/3600/24;
% 
%             %             Upper_latitude_Two_Satellite = sum(visibility_Two_Satellite(91+target_latitude(p),:))/180;
%             %             Lower_latitude_Two_Satellite = sum(visibility_Two_Satellite(91-target_latitude(p),:))/180;
%             Average_visibility_Two_Satellite{iii}(ind,p) = (sum(visibility_Two_Satellite(p,:))/180)*dt*TU/3600/24;
% 
%         end
%         ind = ind + 1;
%     end
% end
% 
% %%
% save('VisibilityData_L145.mat','Average_visibility_One_Satellite','Average_visibility_Two_Satellite');
%%
%%
%%
load('VisibilityData_L145.mat')
%%
% One satellite figure
a=168;
b=181;
for ii = 1:4
    figure; hold on
    ylabel_list = [inc_list(1:orbit_amount:end),inc_list(end)];
    xlabel_list = target_latitude;
    contour(ylabel_list,xlabel_list,Average_visibility_One_Satellite{ii}','ShowText','on','LevelStep',20)
    text_temp = sprintf('\\Theta_{max}=%d',CameraAngleCapability(ii));
    ylabel('Heliographic Latitude [deg]','Interpreter','latex')
    xlabel('Orbital Inclination [deg]','Interpreter','latex')
    set(gca,'FontSize',12)
    ylim([-90,90])
    xlim([0.6,18])
    yticks(-90:30:90)
    xticks(0:5:15)

    for k = 1:a
        for j = 1:b
            if Average_visibility_One_Satellite{ii}(k,j) == 0
                plot(ylabel_list(k),xlabel_list(j),'k','Marker','square','MarkerFaceColor','k','MarkerSize',18)
            end
        end
    end
    
    for h = -90:30:90
        yline(h,'k--');
    end
    for h = 0:5:15
        xline(h,'k--');
    end


    % 
%     figName = sprintf('1sat_%d_deg.pdf',CameraAngleCapability(ii));
%     saveas(gcf,figName)
end

% Two satellite figure
for ii = 2:4
    figure; hold on
    ylabel_list = [inc_list(1:orbit_amount:end),inc_list(end)];
    xlabel_list = target_latitude;
    contour(ylabel_list,xlabel_list,Average_visibility_Two_Satellite{ii}','ShowText','on','LevelStep',20)
    text_temp = sprintf('\\Theta_{max}=%d',CameraAngleCapability(ii));
    ylabel('Heliographic Latitude [deg]','Interpreter','latex')
    xlabel('Orbital Inclination [deg]','Interpreter','latex')
    set(gca,'FontSize',12)
    ylim([-90,90])
    xlim([0.6,18])
    yticks(-90:30:90)
    xticks(0:5:15)

    for k = 1:a
        for j = 1:b
            if Average_visibility_Two_Satellite{ii}(k,j) == 0
                plot(ylabel_list(k),xlabel_list(j),'k','Marker','square','MarkerFaceColor','k','MarkerSize',18)
            end
        end
    end
    
    for h = -90:30:90
        yline(h,'k--');
    end
    for h = 0:5:15
        xline(h,'k--');
    end
%     figName = sprintf('2sat_%d_deg.pdf',CameraAngleCapability(ii));
%     saveas(gcf,figName)
end
for ii = 1
    figure; hold on
    ylabel_list = [inc_list(1:orbit_amount:end),inc_list(end)];
    xlabel_list = target_latitude;
    contour(ylabel_list,xlabel_list,Average_visibility_Two_Satellite{ii}','ShowText','on','LevelStep',5)
    text_temp = sprintf('\\Theta_{max}=%d',CameraAngleCapability(ii));
    ylabel('Heliographic Latitude [deg]','Interpreter','latex')
    xlabel('Orbital Inclination [deg]','Interpreter','latex')
    set(gca,'FontSize',12)
    ylim([-90,90])
    xlim([0.6,18])
    yticks(-90:30:90)
    xticks(0:5:15)

    for k = 1:a
        for j = 1:b
            if Average_visibility_Two_Satellite{ii}(k,j) == 0
                plot(ylabel_list(k),xlabel_list(j),'k','Marker','square','MarkerFaceColor','k','MarkerSize',18)
            end
        end
    end
    
    for h = -90:30:90
        yline(h,'k--');
    end
    for h = 0:5:15
        xline(h,'k--');
    end
%     figName = sprintf('2sat_%d_deg.pdf',CameraAngleCapability(ii));
%     saveas(gcf,figName)
end
% spreadfigures
%%
